function [ Xk, Pk, Kk] = KalmanF( Yk, Xk_1, Pk_1, Phi, H, Qk, Rk )
%   Kalman Filter Fundamental Foemula

    l = length(Xk_1); 
    Xkk_1 = Phi*Xk_1;
    Pkk_1 = Phi*Pk_1*Phi' + Qk;
    Kk = Pkk_1*H'*((H*Pkk_1*H' + Rk)^-1);
    Xk = Xkk_1 + Kk*(Yk - H*Phi*Xk_1);
    Pk = (eye(l) - Kk*H)*Pkk_1*(eye(l) - Kk*H)' + Kk*Rk*Kk';

end